/*
 * Part of the program sensors
 * Feed a queue with data from the ADC
 */
#include <xqueue.h>
#include <xtask.h>
#include <rrcon.h>

#include "sensors_read.h"

namespace sensors_test {
    using namespace kthrobot;

    SensorsReader::SensorsReader()
        : XTask("SensorsReader") {
            SetPeriodMS(period_);
        }

    SensorsReader::~SensorsReader() {
        Join();
    }

    void SensorsReader::TaskCode() {
        while(!MustStop()) {
            // Raw data
            ADCRawData raw_data = RRCon::GetADCDataSlow();
            data_queue_.Enq(raw_data);

            // Data from the gyroscope
            int angle = raw_to_angle_(raw_data.channel[7]);
            angles_queue_.Enq(angle);
            WaitPeriod();
        }
    }

    int SensorsReader::ConvertToCm(int value, int type) {
        // Convert a raw voltage value to a distance
        // type = 0-3 for short-range IR sensors
        // type = 4-5 for long-range IR sensors
        // Return the value in centimers.
        if (type < 4) {
            return ir_calibration_function_(value, 3.5, 16201, 146);
        }
        else {
            return ir_calibration_function_(value, 1.5, 22675, 32);
        }
    }

    int SensorsReader::ir_calibration_function_(
            int V, int k, int m_prime, int b_prime) {
        // Calibration formula for the IR sensors. V is the voltage value given
        // by the ADC converter. The other arguments are the parameters.
        return (m_prime / (V + b_prime)) - k;
    }

    int SensorsReader::raw_to_angle_(int raw_value) {
        // Calibration function to convert the raw value provided by the gyro
        // to an angle, in 100th of degrees.
        int centered_value = raw_value - gyro_offset_;
        // Using a treshold to remove some noise
        if (centered_value < gyro_treshold_ && centered_value > -gyro_treshold_) {
            return 0;
        }
        return centered_value*period_*1000/gyro_scale_;
    }

};
